#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, in1,    Gyro,           sensorGyro)
#pragma config(Sensor, in2,    ArmPot,         sensorPotentiometer)
#pragma config(Sensor, in3,    FlipperPot,     sensorPotentiometer)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_3,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_4,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_5,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_6,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Motor,  port1,           LeftBaseBack,  tmotorVex393HighSpeed, PIDControl, encoder, encoderPort, I2C_1, 1000)
#pragma config(Motor,  port2,           LeftBaseFront, tmotorVex393HighSpeed, PIDControl, encoder, encoderPort, I2C_4, 1000)
#pragma config(Motor,  port3,           RightArm,      tmotorVex393, PIDControl, encoder, encoderPort, I2C_5, 1000)
#pragma config(Motor,  port4,           LeftArm,       tmotorVex393, PIDControl, reversed, encoder, encoderPort, I2C_6, 1000)
#pragma config(Motor,  port5,           LeftConveyor,  tmotorVex269, openLoop, reversed)
#pragma config(Motor,  port6,           RightConveyor, tmotorVex269, openLoop)
#pragma config(Motor,  port7,           flippermotor,  tmotorVex393, openLoop)
#pragma config(Motor,  port8,           turntablemotor, tmotorVex393, openLoop)
#pragma config(Motor,  port9,           RightBaseFront, tmotorVex393HighSpeed, PIDControl, reversed, encoder, encoderPort, I2C_3, 1000)
#pragma config(Motor,  port10,          RightBaseBack, tmotorVex393HighSpeed, PIDControl, reversed, encoder, encoderPort, I2C_2, 1000)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!

////////////////////////////////////////////////////
//			POTENTIOMETER ARM SETTINGS
////////////////////////////////////////////////////
#define ArmsFloor 1060
#define ArmsTrough 2260
#define ArmsHigh 2900
#define ArmsFast 127
#define ArmsSlow 65
#define ArmsMotorHold -10


////////////////////////////////////////////////////
//			ARM TESTING FUNCTIONS
////////////////////////////////////////////////////

// this thread will move the arms to the floor
void MoveArmsToFloor()
{
	// we can only move down when we move to floor position
	while (SensorValue[ArmPot] > ArmsFloor)
	{
		motor(RightArm) = ArmsSlow;
		motor(LeftArm) = ArmsSlow;
	}
	// hold
	motor(RightArm) = 0;
	motor(LeftArm) = 0;
}

// this thread will move the arms UP to the trough
void MoveArmsUpToTrough()
{
	while (SensorValue[ArmPot] < ArmsTrough)
	{
		motor(RightArm) = -ArmsSlow;
		motor(LeftArm) = -ArmsSlow;
	}
	// hold
	motor(RightArm) = ArmsMotorHold;
	motor(LeftArm) = ArmsMotorHold;
}

// this thread will move the arms DOWN to the trough
void MoveArmsDownToTrough()
{
	while (SensorValue[ArmPot] > ArmsHigh)
	{
		motor(RightArm) = ArmsSlow;
		motor(LeftArm) = ArmsSlow;
	}
	// hold
	motor(RightArm) = ArmsMotorHold;
	motor(LeftArm) = ArmsMotorHold;
}

// this thread will move the arms to the high goal
void MoveArmsToHighGoal()
{
	while (SensorValue[ArmPot] < 2000)
	{
		motor(RightArm) = -ArmsSlow;
		motor(LeftArm) = -ArmsSlow;
	}
	// hold
	motor(RightArm) = ArmsMotorHold;
	motor(LeftArm) = ArmsMotorHold;
}

void MoveArm()
{
	motor[RightArm] = -(vexRT(Ch2)/2); // Lifter Motor 1
  motor[LeftArm] = -(vexRT(Ch2)/2); // Lifter Motor 2+-----------------------------
}


/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////

void pre_auton()
{
	// Set bStopTasksBetweenModes to false if you want to keep user created tasks running between
	// Autonomous and Tele-Op modes. You will need to manage all user created tasks if set to false.
	bStopTasksBetweenModes = true;
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task autonomous()
{
	// run these functions to test arm movement
	while(true)
	{
		// we will be in this loop until the user pushes a button
		// check if button has been pressed if so break out of loop
		if (nLCDButtons != 0)
		{
			// somebody pressed a button - break out of this loop and continue on...
			break;
		}
	}
	MoveArmsUpToTrough();
		// run these functions to test arm movement
	while(true)
	{
		// we will be in this loop until the user pushes a button
		// check if button has been pressed if so break out of loop
		if (nLCDButtons != 0)
		{
			// somebody pressed a button - break out of this loop and continue on...
			break;
		}
	}
	MoveArmsToFloor();
		// run these functions to test arm movement
	while(true)
	{
		// we will be in this loop until the user pushes a button
		// check if button has been pressed if so break out of loop
		if (nLCDButtons != 0)
		{
			// somebody pressed a button - break out of this loop and continue on...
			break;
		}
	}
	MoveArmsUpToTrough();
		// run these functions to test arm movement
	while(true)
	{
		// we will be in this loop until the user pushes a button
		// check if button has been pressed if so break out of loop
		if (nLCDButtons != 0)
		{
			// somebody pressed a button - break out of this loop and continue on...
			break;
		}
	}
	MoveArmsToFloor();
		// run these functions to test arm movement
	while(true)
	{
		// we will be in this loop until the user pushes a button
		// check if button has been pressed if so break out of loop
		if (nLCDButtons != 0)
		{
			// somebody pressed a button - break out of this loop and continue on...
			break;
		}
	}
	//MoveArmsToHighGoal();
	//MoveArmsDownToTrough();
}



/////////////////////////////////////////////////////////////////////////////////////////
//                                 User Control Task
/////////////////////////////////////////////////////////////////////////////////////////

task usercontrol()
{
	while (true)
	{
		MoveArm();
	}
}
